
#include <gsl/gsl_pow_int.h>

#include "BodyPart.h"

#include "defs.h"
#include "Creature.h"

#include <iostream>
#include <QtCore/QString>


namespace jcrada {

    BodyPart::BodyPart()
    :
    _id(0),
    _parent_id(Creature::B_NONE),
    _name(""),
    _shape(S_NONE),
    _orientation(Y_AXIS),
    _size_x(1.0),
    _size_y(1.0),
    _size_z(1.0),
    _mass(1.0),
    _max_torque(0.0),
    _size_tolerance(0.00), //1% de tolerancia
    _rigid_body(NULL),
    _collision_shape(NULL),
    _motion_state(NULL) {

    }

    BodyPart::~BodyPart() {
        reset();
    }

    void BodyPart::setSizeX(const btScalar x) {
        switch (getOrientation()) {
            case X_AXIS: this->_size_y = x;
                break;
            case Y_AXIS: this->_size_x = x;
                break;
            case Z_AXIS: this->_size_x = x;
                break;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    btScalar BodyPart::getSizeX() const {
        switch (getOrientation()) {
            case X_AXIS:return this->_size_y;
            case Y_AXIS:return this->_size_x;
            case Z_AXIS:return this->_size_x;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    void BodyPart::setSizeY(const btScalar y) {
        switch (getOrientation()) {
            case X_AXIS: this->_size_x = y;
                break;
            case Y_AXIS: this->_size_y = y;
                break;
            case Z_AXIS: this->_size_z = y;
                break;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    btScalar BodyPart::getSizeY() const {
        switch (getOrientation()) {
            case X_AXIS: return this->_size_x;
            case Y_AXIS: return this->_size_y;
            case Z_AXIS: return this->_size_z;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    void BodyPart::setSizeZ(const btScalar z) {
        switch (getOrientation()) {
            case X_AXIS: this->_size_z = z;
                break;
            case Y_AXIS: this->_size_z = z;
                break;
            case Z_AXIS: this->_size_y = z;
                break;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    btScalar BodyPart::getSizeZ() const {
        switch (getOrientation()) {
            case X_AXIS: return this->_size_z;
            case Y_AXIS: return this->_size_z;
            case Z_AXIS: return this->_size_y;
            default:
                BDEBUG(TO_STRING(getOrientation()));
                assert(0);
        }
    }

    btScalar BodyPart::getSizeXt() const {
        return getSizeX() * (1.0 + getSizeTolerance());
    }

    btScalar BodyPart::getSizeYt() const {
        return getSizeY() * (1.0 + getSizeTolerance());
    }

    btScalar BodyPart::getSizeZt() const {
        return getSizeZ() * (1.0 + getSizeTolerance());
    }

    btScalar BodyPart::getSizeRt() const {
        return getSizeR() * (1.0 + getSizeTolerance());
    }

    void BodyPart::scaleSizeAndMass(const btScalar factor) {
        setSize(getSizeX() * factor, getSizeY() * factor, getSizeZ() * factor);
        setMass(getMass() * factor);
    }

    void BodyPart::set(const btTransform& at) {
        switch (getShape()) {
            case S_BOX:
                setCollisionShape(new btBoxShape(btVector3(
                        getSizeX() / 2.0, getSizeY() / 2.0, getSizeZ() / 2.0)));
                break;
            case S_SPHERE:
                setCollisionShape(new btSphereShape(getSizeR()));
                break;
            case S_CAPSULE_X:
                setCollisionShape(new btCapsuleShapeX(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CAPSULE_Y:
                setCollisionShape(new btCapsuleShape(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CAPSULE_Z:
                setCollisionShape(new btCapsuleShapeZ(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CONE_X:
                setCollisionShape(new btConeShapeX(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CONE_Y:
                setCollisionShape(new btConeShape(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CONE_Z:
                setCollisionShape(new btConeShapeZ(getSizeX() / 2.0, getSizeY()));
                break;
            case S_CYLINDER_X:
                setCollisionShape(new btCylinderShapeX(btVector3(
                        getSizeY() / 2.0, getSizeX() / 2.0, getSizeZ() / 2.0)));
                break;
            case S_CYLINDER_Y:
                setCollisionShape(new btCylinderShape(btVector3(
                        getSizeX() / 2.0, getSizeY() / 2.0, getSizeZ() / 2.0)));
                break;
            case S_CYLINDER_Z:
                setCollisionShape(new btCylinderShapeZ(btVector3(
                        getSizeX() / 2.0, getSizeZ() / 2.0, getSizeY() / 2.0)));
                break;
            default:
                BDEBUG(TO_STRING(getShape()));
                assert(0);
        }
        bool isDynamic = (getMass() != 0.f);

        btVector3 inertia(0, 0, 0);
        if (isDynamic) {
            getCollisionShape()->calculateLocalInertia(getMass(), inertia);
        }

        setMotionState(new btDefaultMotionState(at));

        btRigidBody::btRigidBodyConstructionInfo rbInfo(getMass(), getMotionState(),
                getCollisionShape(), inertia);
        //        rbInfo.m_friction = 0;
        //        getRigidBody()->setFriction(0.5);
        //        getRigidBody()->setRestitution(0.0);
        setRigidBody(new btRigidBody(rbInfo));
        getRigidBody()->setActivationState(DISABLE_DEACTIVATION);
    }

    void BodyPart::reset() {
        if (getRigidBody()) {
            delete getRigidBody();
        }
        if (getMotionState()) {
            delete getMotionState();
        }
        if (getCollisionShape()) {
            delete getCollisionShape();
        }
    }

    BodyPart* BodyPart::clone() const {
        BodyPart* result = new BodyPart();
        result->setId(getId());
        result->setMass(getMass());
        result->setMaxTorque(getMaxTorque());
        result->setName(getName());
        result->setOrientation(getOrientation());
        result->setParentId(getParentId());
        result->setShape(getShape());
        result->setSize(getSizeX(), getSizeY(), getSizeZ());
        result->setSizeTolerance(getSizeTolerance());
        return result;
    }

    btScalar BodyPart::getVolume() const {
        return GET_VOLUME(getShape(), getSizeX(), getSizeY(), getSizeZ(),getSizeR());
    }

    btScalar BodyPart::GET_VOLUME(eShape shape, btScalar x, btScalar y, btScalar z, btScalar r) {
        switch (shape) {
            case S_BOX: return x * y * z;
            case S_SPHERE: return (4.0 / 3.0) * M_PI * gsl_pow_3(r);
            case S_CYLINDER_X:
            case S_CYLINDER_Y:
            case S_CYLINDER_Z: return M_PI * gsl_pow_2(r) * y;
            case S_CONE_X:
            case S_CONE_Y:
            case S_CONE_Z: return (1.0 / 3.0) * M_PI * gsl_pow_2(r) * y;
            case S_CAPSULE_X:
            case S_CAPSULE_Y:
            case S_CAPSULE_Z: return GET_VOLUME(S_SPHERE, x, y, z, r) + GET_VOLUME(S_CYLINDER_X, x, y, z, r);
            default: assert(0);
        }
    }
    //-----------------------------------------------------------
    //XML
    //-----------------------------------------------------------

    BodyPart* BodyPart::fromXml(const QDomElement& xml, bool* ok) {
        BodyPart* result = new BodyPart();
        result->setId(xml.attribute("id", "0").toInt(ok));
        result->setParentId(xml.attribute("parent-id", "0").toInt(ok));
        result->setName(xml.attribute("name", "").toStdString());
        result->setMass(xml.attribute("mass", "1.0").toDouble(ok));
        result->setShape((eShape) xml.attribute("shape", QString::number(BodyPart::S_NONE)).toInt(ok));
        result->setSizeX(xml.attribute("size-x", "1.0").toDouble(ok));
        result->setSizeY(xml.attribute("size-y", "1.0").toDouble(ok));
        result->setSizeZ(xml.attribute("size-z", "1.0").toDouble(ok));
        result->setSizeTolerance(xml.attribute("size-tolerance", "0.1").toDouble());
        if (!*ok) {
            BDEBUG("Error in BodyPart::fromXml");
        }
        return result;
    }

    void BodyPart::toXml(QDomDocument& xml, QDomElement* parent) const {
        QDomElement body = xml.createElement("BodyPart");
        body.setAttribute("id", getId());
        body.setAttribute("parent-id", getParentId());
        body.setAttribute("name", QString(getName().c_str()));
        body.setAttribute("mass", getMass());
        body.setAttribute("shape", getShape());
        body.setAttribute("size-x", getSizeX());
        body.setAttribute("size-y", getSizeY());
        body.setAttribute("size-z", getSizeZ());
        body.setAttribute("size-tolerance", getSizeTolerance());

        if (parent) {
            parent->appendChild(body);
        } else {
            xml.appendChild(body);
        }
    }

    std::string BodyPart::toXml() const {
        QDomDocument doc;
        this->toXml(doc);
        return doc.toString().toStdString();
    }
}
